import sys
import argparse
sys.path.append("..") 
from utilities import connect_to_sysid

from pymavlink import mavutil

def reboot(mav_connection):
    mav_connection.wait_heartbeat()
    print("Heartbeat from system (system %u component %u)" %
          (mav_connection.target_system, mav_connection.target_component))
    mav_connection.mav.command_long_send(mav_connection.target_system, mav_connection.target_component,
                                         mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0, 1, 0, 0, 0, 0, 0, 0)
    msg = mav_connection.recv_match(type='COMMAND_ACK', blocking=True)
    print(msg)

    # return the result of the ACK message
    return msg.result

if __name__ == "__main__":
    parser = argparse.ArgumentParser(description='Reboot fcu using mavlink.')
    parser.add_argument('-c', '--connect', help="Connection string", default='/dev/ttyACM0')
    parser.add_argument('-s', "--sysid", type=int, help="System ID of the UAV to command.", default=1)
    

    args = parser.parse_args()

    mav_connection = connect_to_sysid(args.connect, args.sysid)
    result = reboot(mav_connection)
    print(f'Result of reboot command: {result}')